#include <cv.h>
#include <cxcore.h>
#include <highgui.h>

const int MAX_CORNERS = 500;

int main(int argc,char** argv)
{
	IplImage* imgA = cvLoadImage(".\/image0.jpg",CV_LOAD_IMAGE_GRAYSCALE);
	IplImage* imgB = cvLoadImage(".\/image1.jpg",CV_LOAD_IMAGE_GRAYSCALE);
	assert(imgA);
	assert(imgB);
	CvSize img_sz = cvGetSize(imgA);
	int win_size = 10;

	IplImage* eig_image = cvCreateImage(img_sz,IPL_DEPTH_32F,1);
	IplImage* tmp_image = cvCreateImage(img_sz,IPL_DEPTH_32F,1);

	int corner_count = MAX_CORNERS;

	CvPoint2D32f *cornersA = new CvPoint2D32f[MAX_CORNERS];

	cvGoodFeaturesToTrack(imgA,eig_image,tmp_image,cornersA,&corner_count,
		0.01,
		5.0,
		0,
		3,
		0,
		0.04
		);
	
	cvFindCornerSubPix(imgA,cornersA,corner_count,cvSize(win_size,win_size),cvSize(-1,-1),
		cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)
		);
//Lucas Kanade algotithm
	char feature_found[MAX_CORNERS];
	float feature_errors[MAX_CORNERS];
	CvSize pyr_sz = cvSize(imgA->width+8,imgB->height/3);

	IplImage * pyrA = cvCreateImage(pyr_sz,IPL_DEPTH_32F,1);
	IplImage * pyrB = cvCreateImage(pyr_sz,IPL_DEPTH_32F,1);

	CvPoint2D32f* cornersB = new CvPoint2D32f[MAX_CORNERS];

	cvCalcOpticalFlowPyrLK(imgA,imgB,pyrA,pyrB,cornersA,cornersB,corner_count,
		cvSize(win_size,win_size),5,feature_found,feature_errors,
		cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,.3),
		0
		);

// view the picture
	for(int i=0;i<corner_count;i++)
	{
		if((feature_found[i]==0)||(feature_errors[i]>550))
		{
			printf("Error is %f\n",feature_errors[i]);
			continue;
		}
		printf("Got it \n");
		IplImage *imgC = cvCreateImage(img_sz,IPL_DEPTH_32F,3);
		cvSet(imgC,CV_RGB(255,255,255));
		CvPoint p0 = cvPoint(
			cvRound(cornersA[i].x),cvRound(cornersA[i].y));
		CvPoint p1 = cvPoint(
			cvRound(cornersB[i].x),cvRound(cornersB[i].y));
		cvLine(imgC,p0,p1,CV_RGB(255,0,0),2);
		cvNamedWindow("ImageA",0);
		cvNamedWindow("ImageB",0);
		cvNamedWindow("OpticalFlow",0);
		cvShowImage("ImageA",imgA);
		cvShowImage("ImageB",imgB);
		cvShowImage("OpticalFlow",imgC);
		cvWaitKey(0);
		return 0;
	}

}